#include "stdio.h"
#include "math.h"
#include "track.h"


void updateQuaternionGyr(Quaternion *q_new, float norm_gyr, float gx, float gy, float gz, float deltaT) {
    Quaternion result;
    if (norm_gyr < 1e-8f)  {
        // norm_gyr = 1;
        setQuaternion(q_new, 1.0f, 0.0f, 0.0f, 0.0f);
        return;
    }
    Quaternion q_del;
    setFromAngleAxis(&q_del, deltaT * norm_gyr, gx / norm_gyr,gy / norm_gyr,gz / norm_gyr);
    multiply(q_new, &q_del, &result);
    normalize(&result);
    setQuaternion(q_new, result.q0, result.q1, result.q2, result.q3);
}

void updateQuaternionComp(Quaternion *q, float norm_gyr, float gx, float gy, float gz, float ax, float ay, float az, float deltaT){

    // Quaternion q_new, q_acc,q_accel, q_accel_w, tilt;
    // float phi, alpha, beta;
    // setQuaternion(&q_new, q->q0, q->q1, q->q2, q->q3);

    // 计算 在当前姿态q的基础上,单纯叠加了陀螺仪角度增量的q_new
    updateQuaternionGyr(q, norm_gyr, gx, gy, gz, deltaT);

    // setQuaternion(&q_accel, 0, ax, ay, az);
    // setQuaternion(&q_acc, 0, ax, ay, az);

    // beta = length(&q_accel);

    // if(beta > 0.97 && beta < 1.03) {
    //     /* function to rotate a quaternion by q_new * q_acc * q_new^{-1} */
    //     rotate(&q_new, &q_accel, &q_accel_w);
    //     normalize(&q_accel_w);

    //     phi = acosf(fabsf(q_accel_w.q1)) * 180.f / PI;
    //     if(q_accel_w.q1 > 0.0f){
    //         phi = 180.f - phi;
    //     }
    //     float aixs_y = q_accel_w.q1*q_accel_w.q3;
    //     float aixs_z = -q_accel_w.q1*q_accel_w.q2;
    //     float n = sqrtf(aixs_y*aixs_y + aixs_z*aixs_z);
    //     float ny = aixs_y / n;
    //     float nz = aixs_z / n;

    //     alpha = 0.8f;
    //     setFromAngleAxis(&tilt,(1.f-alpha)* phi, 0.0f, ny, nz);
    //     multiply(&tilt, &q_new, q);
    // }else{
    //     setQuaternion(&tilt, 1.0f,0.0f,0.0f,0.0f);
    //     multiply(&tilt, &q_new, q);
    // }
}

Pose3 quaternion2EularAngles(Quaternion q)
{
        Pose3 pose;
        double test = q.q2 * q.q0 + q.q1 * q.q3;

        if((float)test > 0.4999f) {
            pose.roll = (double)2.0 * atan2(q.q2, q.q0);
            pose.pitch = PI/2;
            pose.yaw = 0.0;
            return pose;
        }

        if((float)test < -0.4999f) {
            pose.roll = (double)2.0 * atan2(q.q2, q.q0);
            pose.pitch = -PI/2;
            pose.yaw = 0.0;
            return pose;
        }

        double sqx = q.q1 * q.q1;
        double sqy = q.q2 * q.q2;
        double sqz = q.q3 * q.q3;

        pose.yaw = atan2((double)2.0 * q.q1 * q.q0 - (double)2.0 * q.q3 * q.q2, (double)1.0 - (double)2.0 * sqy - (double)2.0 * sqx);
        pose.roll = asin((double)2.0 * test);
        pose.pitch = atan2((double)2.0 * q.q3 * q.q0 - (double)2.0 * q.q2 * q.q1, (double)1.0 - (double)2.0 * sqy - (double)2.0 * sqz);

        return pose;
}